// matrix_implementation_definition.h
#ifndef MATRIX_IMPLEMENTATION_DEFINITION_H
#define MATRIX_IMPLEMENTATION_DEFINITION_H

#include "matrix_implementation.h"

#include <iomanip>
#include <memory>
#include <algorithm>

template<typename T, typename ElementTrait>
MatrixImplementation<T,ElementTrait>::MatrixImplementation(size_t dim) : 
    _dim(dim),
    _d(0)
{
    checkDim(_dim);
    std::unique_ptr<T[]> dWrapper(new T[dim*dim]());
    // Uncomment below to see effect of unique_ptr - avoiding memory leaks
    //throw std::exception();
    _d = dWrapper.release();
}
    
template<typename T, typename ElementTrait>
MatrixImplementation<T,ElementTrait>::MatrixImplementation(MatrixImplementation<T,ElementTrait> const & toCopy): 
    _dim(toCopy._dim),
    _d(0)
{
    std::unique_ptr<T[]> dWrapper(new T[toCopy._dim*toCopy._dim]());
    _d = dWrapper.get();
    std::copy_n(toCopy._d,_dim*_dim,_d);
    dWrapper.release();
}

template<typename T, typename ElementTrait>
MatrixImplementation<T,ElementTrait>::MatrixImplementation(MatrixImplementation<T,ElementTrait> && toMove)
{
    _dim = toMove._dim;
    _d = toMove._d;
    toMove._d = 0;
}

template<typename T, typename ElementTrait>
MatrixImplementation<T,ElementTrait>::~MatrixImplementation()
{
    delete[] _d;
}

template<typename T, typename ElementTrait>
bool MatrixImplementation<T,ElementTrait>::IsIdentity() const
{
    for(size_t i = 0; i < _dim; ++i)
    {
        for(size_t j = 0; j < _dim; ++j)
        {
            T r = i == j ? _d[i*_dim+j] - T(1) : _d[i*_dim+j];
            if (ElementTrait::Abs(r) > ElementTrait::Precision)
            {
                return false;
            }
        }
    }

    return true;
}

template<typename T, typename ElementTrait>
void MatrixImplementation<T,ElementTrait>::Swap(MatrixImplementation<T,ElementTrait> & other)
{
    T * tempD = _d;
    _d = other._d;
    other._d = tempD;
    
    size_t tempDim = _dim;
    _dim = other._dim;
    other._dim = tempDim;
}

template<typename T, typename ElementTrait>
MatrixImplementation<T,ElementTrait> const MatrixImplementation<T,ElementTrait>::Invert() const throw (MatrixException)
{
    static_assert(ElementTrait::IsInvertable,"Element type T does not support inversion of matrix");
    MatrixImplementation<T,ElementTrait> result(_dim);
    // Using unique_ptr to ensure deallocation of temporary pointer
    std::unique_ptr<T[]> temp(new T[_dim*_dim]());
    inv_mat(_d,result._d,temp.get(),_dim);
    return std::move(result);
}

template<typename T, typename ElementTrait>
size_t const MatrixImplementation<T,ElementTrait>::Dimension() const
{
    return _dim;
}

template<typename T, typename ElementTrait>
T const * MatrixImplementation<T,ElementTrait>::GetInternal() const
{
    return _d;
}
template<typename T, typename ElementTrait>
T & MatrixImplementation<T,ElementTrait>::operator() (size_t row, size_t col) throw (MatrixException)
{
    if(row >= _dim || col >= _dim)
    {
        throw MatrixException("Invalid row or column for element access");
    }
    return _d[row+col*_dim];
}

template<typename T, typename ElementTrait>
T const MatrixImplementation<T,ElementTrait>::operator() (size_t row, size_t col) const throw (MatrixException)
{
    if(row >= _dim || col >= _dim)
    {
        throw MatrixException("Invalid row or column for element access");
    }
    return _d[row+col*_dim];
}

template<typename T, typename ElementTrait>
MatrixImplementation<T,ElementTrait> & MatrixImplementation<T,ElementTrait>::AddTo (MatrixImplementation<T,ElementTrait> const & other) throw (MatrixException) 
{
    checkDim(*this,other);

    for(size_t i = 0 ; i < _dim*_dim ; ++i)
    {
        _d[i] += other._d[i];
    }

    return *this;
}

template<typename T, typename ElementTrait>
MatrixImplementation<T,ElementTrait> const MatrixImplementation<T,ElementTrait>::Add (MatrixImplementation<T,ElementTrait> const & other) const throw (MatrixException) 
{
    return std::move(MatrixImplementation<T,ElementTrait>(*this).AddTo(other));
}

template<typename T, typename ElementTrait>
MatrixImplementation<T,ElementTrait> & MatrixImplementation<T,ElementTrait>::MultiplyTo (MatrixImplementation<T,ElementTrait> const & other) throw (MatrixException)
{
    MatrixImplementation<T,ElementTrait> temp = Multiply(other);
    Swap(temp);
    return *this;
}

template<typename T, typename ElementTrait>
MatrixImplementation<T,ElementTrait> const MatrixImplementation<T,ElementTrait>::Multiply (MatrixImplementation<T,ElementTrait> const & other) const throw (MatrixException)
{
    checkDim(*this,other);

    MatrixImplementation<T,ElementTrait> result(_dim);
    mat_mult(_d,other._d,result._d,_dim);

    return std::move(result);
}

template<typename T, typename ElementTrait>
template<typename U>
MatrixImplementation<T,ElementTrait> & MatrixImplementation<T,ElementTrait>::MultiplyTo (U const & other)
{
    for(size_t i = 0; i < _dim; ++i)
    {
        _d[i] *= other;
    }

    return *this;
}

template<typename T, typename ElementTrait>
template<typename U>
MatrixImplementation<T,ElementTrait> const MatrixImplementation<T,ElementTrait>::Multiply (U const & other) const 
{
    return std::move(MatrixImplementation<T,ElementTrait>(*this).MultiplyTo(other));
}

template<typename T, typename ElementTrait>
bool MatrixImplementation<T,ElementTrait>::Equals (MatrixImplementation<T,ElementTrait> const & other) const
{
    if(other._dim != _dim)
    {
        return false;
    }
    for(size_t i = 0 ; i < _dim*_dim ; ++i)
    {
        if(ElementTrait::Abs(_d[i] - other._d[i]) > ElementTrait::Precision)
        {
            return false;
        }
    }

    return true;
}

template<typename T, typename ElementTrait>
MatrixImplementation<T,ElementTrait> MatrixImplementation<T,ElementTrait>::Identity(size_t dim)
{
    MatrixImplementation<T,ElementTrait> res(dim);
    for(size_t i = 0 ; i < dim ; ++i)
    {
        res(i,i) = T(1);
    }
    return std::move(res);
}

template<typename T, typename ElementTrait>
MatrixImplementation<T,ElementTrait> MatrixImplementation<T,ElementTrait>::RandomMatrix(size_t dim)
{
    MatrixImplementation<T,ElementTrait> res(dim);
    gen_mat(res._d,dim);
    return std::move(res);
}

template<typename T, typename ElementTrait>
void MatrixImplementation<T,ElementTrait>::checkDim(size_t dim) throw (MatrixException)
{
    if (dim == 0) throw MatrixException( "Illegal dimension.");
}

template<typename T, typename ElementTrait> 
void MatrixImplementation<T,ElementTrait>::checkDim(MatrixImplementation<T,ElementTrait> const & a, MatrixImplementation<T,ElementTrait> const & b) throw (MatrixException)
{
    if(a._dim != b._dim) throw MatrixException("Incompatible dimensions.");
}


///////////////////////////////////////////////////////////////////////////////////////////
// Below is basically copy-paste from the assingment, with added template arguments
// and throwing exceptions instead of aborting
///////////////////////////////////////////////////////////////////////////////////////////

template<typename T, typename ElementTrait>
void MatrixImplementation<T,ElementTrait>::checkDivider(T x) throw (MatrixException)
{
    if (ElementTrait::Precision > ElementTrait::Abs(x))
    {
        throw MatrixException("Illegal divider.");
    }
}

/* Read a re-arranged matrix. */
#define A(I,J) a[index[I]*n+(J)]
#define B(I,J) b[index[I]*n+(J)]

/* a = invert(input), using b as temporary matrix
 * Do not mind the signature, this is old code.
 * You do not need to look at the implementation in detail.
 */
template<typename T, typename ElementTrait>
void MatrixImplementation<T,ElementTrait>::inv_mat(const T *input, T *a, T *b, size_t n) throw (MatrixException)
{
    std::unique_ptr<size_t[]> indexWrapper(new size_t[n]);
    size_t * index = indexWrapper.get();

    // Init a, b, and index.
    memcpy(a, input, n*n*sizeof(T));
    //initializeData(b,n);
    for(size_t i = 0; i < n; ++i)
    {
        index[i] = i;   // Identity mapping.
        b[i*n+i] = T(1); // Identity matrix.
    }

    // Gaussian elimination.
    for(size_t k = 0; k < n; ++k)
    {
        // Find pivot.
        T x = ElementTrait::Abs(A(k,k));
        size_t j = k;
        for(size_t i = k+1; i < n; ++i)
        {
            T y = ElementTrait::Abs(A(i,k));
            if (y > x)
            {
                j = i;
                x = y;
            }
        }

        // Swap j <-> k.
        size_t i = index[j];
        index[j] = index[k];
        index[k] = i;

        // Division step.
        x = A(k,k);
        A(k,k) = T(1);
        checkDivider(x);
        for(size_t j = k+1; j < n; ++j) A(k,j) /= x;
        for(size_t j = 0  ; j < n; ++j) B(k,j) /= x;

        // Elimination step.
        for(i = k+1; i < n; ++i)
        {
            for(j = k+1; j < n; ++j) A(i,j) -= A(i,k)*A(k,j);
            for(j = 0  ; j < n; ++j) B(i,j) -= A(i,k)*B(k,j);
            A(i,k) = T();
        }
    }

    // Back-substitution.
    for(size_t k = n-1; k > 0; --k)
    {
        for(size_t i = k-1; (int)i >= 0; --i)
        {
            for(size_t j = 0; j < n; ++j) B(i,j) -= A(i,k)*B(k,j);
            //A(i,k) = 0.0; -- implicit and not used later.
        }
    }

    // Result.
    for(size_t i = 0; i < n; ++i)
    {
        for(size_t j = 0; j < n; ++j)
        {
            a[i*n+j] = B(i,j);
        }
    }
}

template<typename T, typename ElementTrait>
void MatrixImplementation<T,ElementTrait>::gen_mat(T *a, size_t dim)
{
    using namespace std;
    srand(dim);

    for(size_t i = 0; i < dim; ++i)
    {
        for(size_t j = 0; j < dim; ++j)
        {
            T z = ElementTrait::GetRandom();
            a[i*dim+j] = (ElementTrait::Abs(z) * T(10) > ElementTrait::Precision ) ? z : T();
        }
    }
}

template<typename T, typename ElementTrait>
void MatrixImplementation<T,ElementTrait>::mat_mult(T const * a, T const * b, T * c, size_t dim)
{
    for(size_t i = 0; i < dim; ++i)
    {
        for(size_t j = 0; j < dim; ++j)
        {
            c[i*dim+j] = T();
        }
        for(size_t k = 0; k < dim; ++k)
        {
            for(size_t j = 0; j < dim; ++j)
            {
                c[i*dim+j] += a[i*dim+k]*b[k*dim+j];
            }
        }
    }  
}

#endif // MATRIX_IMPLEMENTATION_DEFINITION_H
